import numpy as np
import math
import matplotlib.pyplot as plt

class transition:
    def __init__(self,startvel=0):
        self.maxvel = 0.5
        self.adjvel = 0.5
        self.cornervel = 0.5
        self.translen = 0.156
        self.controlpoints = []
        self.bline = False
        self.state = 0
        self.time = 0
        self.startvel=startvel
        self.exitvel=startvel

    def transpattern(self, p, d):
        self.segments=d
        self.direction=[]
        self.direction.append(np.asarray(d[0]/np.linalg.norm(d[0])))
        self.direction.append(np.asarray(d[1]/np.linalg.norm(d[1])))
        self.P=[]
        if(np.cross(d[0],d[1])==0):
            self.bline=True
            self.P.append(p-d[0])
            self.P.append(p)
            self.P.append(p+d[1])
            return
        self.P.append(p-d[0])
        self.P.append(p-self.translen*d[0]/np.linalg.norm(d[0]))
        self.P.append(p+self.translen*d[1]/np.linalg.norm(d[1]))
        self.P.append(p+d[1])
        beta= math.acos(np.dot(np.asarray(d[0]),np.asarray(d[1]).T)/np.linalg.norm(d[0])/np.linalg.norm(d[1]))/2
        c1=0.4*(np.sqrt(6.0)-1)
        c2=(c1+4)*(c1+1)
        c3=(c1+4)/(c2+6)
        c4 = 1.0 / (1 - c1 * c3 - c3)
        c5 = (c2 + 4) *(c2 + 4) / (54 * c3)
        yita = 6 * c3 * math.cos(beta) / (c1 + 4)
        B=[]
        B.append(p-self.direction[0] *self.translen)
        B.append(p-self.direction[0]*(1-c1*c3)*self.translen)
        B.append(p-self.direction[0]*(1-c1*c3-c3)*self.translen)
        B2=[]
        B2.append(p + self.direction[1] * self.translen)
        B2.append(p + self.direction[1] * (1 - c1 * c3) *self.translen)
        B2.append(p + self.direction[1] * (1 - c1 * c3 - c3) *self.translen)
        ud = (B2[2] - B[2]) / np.linalg.norm((B2[2] - B[2]))

        B.append(B[2]+yita * self.translen * ud)
        B2.append(B2[2]-yita * self.translen * ud)
        Bt=[]
        Bt.append(B2[3])
        Bt.append(B2[2])
        Bt.append(B2[1])
        Bt.append(B2[0])

        self.controlpoints.append(B)
        self.controlpoints.append(Bt)


    def evalcurve(self,n):
        w=np.zeros((2*n,2))
        for i in range(0,n):
            w[i]= self.bezierCurve(0, i*1.0 / n )
            w[n+i]= self.bezierCurve(1, i*1.0 / n)
        return w

    def bezierCurve(self,index,t):
        w = 0
        p = 3
        pc = self.controlpoints[index]
        for i in range(1,(p + 2)):
            w = w + math.factorial(p)/math.factorial(i-1)/math.factorial(p+1-i) * pc[i-1]* np.power(t, (i-1)) * np.power((1-t) , (p+1-i))
        #print w
        return w

    def interpolate(self,dt=0.01):
        trj=[]
        t=0
        curvelength=0.2575
        te=np.sqrt(np.sqrt(3)/3*abs(self.maxvel-self.startvel))
        p=self.P[0]
        while(t<te):
            p=self.startvel*t+2.5*(self.maxvel-self.startvel)*np.power(t,4)/np.power(te,3)\
                -3*(self.maxvel-self.startvel)*np.power(t,5)/np.power(te,4)+(self.maxvel-self.startvel)*np.power(t,6)/np.power(te,5);
            p=self.P[0] + p*self.direction[0]
            trj.append(p)
            t=t+dt
        Pt=p
        L1=(self.startvel+self.maxvel)/2*te
        te=np.sqrt(np.sqrt(3)/3*abs(self.maxvel-self.cornervel))
        L2=(self.cornervel+self.maxvel)/2*te
        if self.bline:
            Lc=np.linalg.norm(self.segments[0])-L1
        else:
            Lc=(np.linalg.norm(self.segments[0])-L1-L2-self.translen)
        while(np.linalg.norm(p-Pt)<Lc):
            p=p+self.maxvel*dt*self.direction[0]
            trj.append(p)
        if self.bline:
            self.exitvel=self.maxvel
            return trj
        t= dt - (L1+Lc-np.linalg.norm(p-self.P[0]))/self.maxvel
        Pt=self.P[0]+self.direction[0]*(L1+Lc)
        while(t<te):
            p=self.maxvel*t+2.5*(self.cornervel-self.maxvel)*np.power(t,4)/np.power(te,3)\
                -3*(self.cornervel-self.maxvel)*np.power(t,5)/np.power(te,4)+(self.cornervel-self.maxvel)*np.power(t,6)/np.power(te,5);
            p=Pt + p*self.direction[0]
            trj.append(p)
            t=t+dt
        #trj.pop()
        Pt=p
        ds=self.cornervel*dt
        du0=ds/(curvelength/2)
        #return trj

        u=0;
        p1=Pt
        while(u<1):
            e=1;  ut=u+du0;
            while(e>10e-5):
                du=ut-u;
                w=self.bezierCurve(0,ut)
                #print w,trj[-1]
                if(len(trj)<1):
                    dst=w-self.P[0]
                else:
                    dst=w-trj[-1];
                dstl=np.linalg.norm(dst)
                e=ds-dstl;
                de=(dstl/du)
                #print "du is %f, dsl is %f,ds is %f, and ut is %f"%(du,dstl,e,ut)
                ut=ut+0.2*e/(de);
            #print ut
                if(ut<=u):
                    ut=u+du0/10
                if(ut>u+du0*3):
                    ut=u+du0*3
            u=ut
            trj.append(self.bezierCurve(0,u));

        trj.pop()

        u=0
        dsr=np.linalg.norm(self.bezierCurve(0,1)-trj[-1]);
        while(u<1):
            e=1;  ut=u+du0
            while(e>10e-5):
                du=ut-u
                w=self.bezierCurve(1,ut)
                dst=w-trj[-1]
                dstl=np.linalg.norm(dst)
                e=ds-dstl
                de=(dstl/du)
                ut=ut+0.2*e/(de)
                #print "du is %f, dsl is %f,ds is %f, and ut is %f"%(du,dstl,e,ut)
            if(ut<=u):
                ut=u+du0/10
            if(ut>u+du0*3):
                ut=u+3*du0
            u=ut
            trj.append(self.bezierCurve(1,u))
        self.exitvel=self.cornervel
        return trj
            



if __name__=="__main__":
    d=[]
    d.append(np.asarray([0.5,0]))
    d.append(np.asarray([0,0.5]))
    bes=transition()
    bes.transpattern([1.0,0],d)
    w=bes.evalcurve(100)
    #w=bes.interpolate()
    plt.figure()
    plt.plot([x[0] for x in w],[x[1] for x in w],'r')
    plt.plot([1,1],[0,0.5])
    plt.plot([0.5,1],[0,0])
    #plt.plot([x[1] for x in w],'r')

    #plt.plot(w)
    # plt.plot(w[:, 0], w[:, 1])
    # plt.plot([0,1,1], [0,0,1],'r')
    plt.show()